// SPDX-License-Identifier:	GPL-2.0+
#include <common.h>
#include <dm.h>
#include <clk.h>
#include <clk-uclass.h>
#include <asm/io.h>
#include <div64.h>

#include "ls7a_init_lib.h"
#include "ls7a_gmem_param.h"

#define CLOCK_AS_DEFAULT  0
#define EFI_LS7A_PCIE_X1_MODE 1
#define EFI_LS7A_PCIE_X4_MODE 4
#define EFI_LS7A_PCIE_X8_MODE 8

#define OLD_BP

#define HIGH_MEM_BASE_ADDRESS 0x9800000090000000

#ifdef DEBUG
#define PRINT_HT1_REG
#define ls7a_debug printf
#else
#define ls7a_debug(fmt, args...)
#endif

static int raw_memsz;

extern void DDR3Leveling(u64 RegsBase, u64 NodeId, u8 ModuleType, u8 NumSlice, u8 RawCardVersion);

unsigned int Abs(unsigned int a1, unsigned int a2)
{
	return a1 > a2 ? (a1 - a2) : (a2 - a1);
}

u16 StatVsCap(u32 Base, u32 Bus)
{
	u64 Addr  = 0;
	u32 CapID = 0, NextCap = 0, Val32 = 10000;
	u16 DevWidthCap  = 0,DevSpeedCap = 0, DevLinkWidth = 0, DevLinkSpeed = 0;
	u16 CtrlSpeedCap = 0, CtrlWidthCap = 0, SpeedCap = 0, WidthCap = 0, Count = 0, ErrNum = 0;

	while (--Val32) {
		NextCap = Readl(0x90000efe10000000 + (Bus << 16) + 0x34) & 0xff;
		udelay(10);
		if ((NextCap != 0) && (NextCap != 0xff)) {
			Count++;
			if (Count > 100) {
				break;
			}
		}
	}
	if (Val32 == 0) {
		return 1;
	}

	while (CapID != 0x10) {
		if (ErrNum >= 100) {
			return 1;
		}
		Addr = 0x90000efe10000000 + (Bus << 16) + NextCap;
		Val32 = Readl(Addr);
		NextCap = (Val32 >> 8) & 0xff;
		CapID = Val32 & 0xff;
		if (CapID == 0xff) { /*invaild val*/
			ErrNum++;
		}
	}
	/* Get the Cap and status of Device */
	DevLinkWidth = (Read16(Addr + 0x12) >> 4) & 0x3f;
	DevLinkSpeed = (Read16(Addr + 0x12)) & 0xf;

	DevWidthCap = (Read16(Addr + 0xc) >> 4) & 0x3f;
	DevSpeedCap = (Read16(Addr + 0xc)) & 0xf;

	/* Get the Cap of pcie controller */
	CtrlWidthCap = (Read16(0x90000efe00000000 + Base + 0x7c) >> 4) & 0x3f;
	CtrlSpeedCap = (Read16(0x90000efe00000000 + Base + 0x7c)) & 0xf;

	/*Compare the cap between  device  and  the controller*/
	WidthCap = DevWidthCap >= CtrlWidthCap? CtrlWidthCap: DevWidthCap;
	SpeedCap = DevSpeedCap >= CtrlSpeedCap? CtrlSpeedCap: DevSpeedCap;

	if ((DevLinkWidth < WidthCap) || (DevLinkSpeed < SpeedCap)) {
		/* abnormal*/
		return 1;
	} else {
		/*normal*/
		return 0;
	}
}

void Ls7aGetPcieDllScore(u64 ConfregAddr, CurrentScore *Result)
{
	unsigned int Control, Compare, ReadConfreg, Loop;
	Result->Clk1 = 0;
	Result->Clk0 = 0;
	Result->CurrentScore = PCIE_PD_LOOP * 2;
	Loop = PCIE_PD_LOOP;
	do {
		ReadConfreg = Readl(ConfregAddr);
		Compare = ((ReadConfreg >> 24 ) & 0xf);
		Control = 0;
		do {
			switch(Compare){
			case 0x3:
				Result->Clk1 += (1 << 0);
				Result->CurrentScore -= 1;
				break;
			case 0xc:
				Result->Clk0 += (1 << 0);
				Result->CurrentScore -= 1;
				break;
			case 0x1:
			case 0x4:
			case 0x5:
				Result->CurrentScore += 1;
				break;
			default:
				break;
			}
			if (Control != 0) {
				break;
			}
			Control++;
			Compare = ((ReadConfreg >> 28 ) & 0xf);
		} while(1);
		Loop--;
	} while(Loop);
}

int Ht3aSideConfigure (u64 Base, u8 NodeId)
{
	u32 Memsize, Val32;
#if 0  //enable HT sample for debug
	Writel(0x54, 0x1);
#endif

	/* open rx space in cpu
	* window0 00_0000_0000 - 1f_ffff_ffff
	* cache DMA and uncache DMA are mutual exclusion or co-work together???
	* */
	ls7a_debug("open 3A ht rx space\n");
	//translate 0x80000000 ~ 0x90000000 to 0x0 ~ 0x10000000 for legacy kernel
	Writel(Base + LS3A_HT_RX_CACHE_WIN0_OFFSET + 4, 0x0080fff0);
	Writel(Base + LS3A_HT_RX_CACHE_WIN0_OFFSET, 0xc0000000);

	// Memsize : 0x1000 = 4G  0x2000 = 8G  0x4000 = 16G ...
	// Resave Node 0 High Mem 512M to Gpu
	Memsize = raw_memsz; //(PcdGet64(PcdNode0MemorySize)>>20);
	ls7a_debug("Memsize=0x%x\n",Memsize);
	//TODO There is no nessery to open Node1 GPU GMEM window.
	//Get GPU GMEM window Base Addr if High mem base addr is 0x90000000 below Val32 should is 0x6
	Val32 = (u32)(((((HIGH_MEM_BASE_ADDRESS&0xfffffffffULL)>>28)&0xff) - 0x3)<<4);
	Val32 |= 0xc0000000;
	Val32 += (Memsize >> 0x4);
	//translate 0x20000000 ~ 0x40000000 to (0xe0000000 + (Memsize << 20)) ~ (0x100000000 + (Memsize << 20))
	Writel(Base + LS3A_HT_RX_CACHE_WIN1_OFFSET + 4, 0x0020ffe0);
	Writel(Base + LS3A_HT_RX_CACHE_WIN1_OFFSET, Val32);

	//open rx window: 0x0 ~ 0x3f_ffff_ffff
	Writel(Base + LS3A_HT_RX_CACHE_WIN2_OFFSET + 4, 0x0000c000);
	Writel(Base + LS3A_HT_RX_CACHE_WIN2_OFFSET, 0x80000000);
#ifdef MULTI_CHIP
	//related to system chip number, not the 3A - 7A connetion
	//recover node 1 access address (bit 37: node id)
#ifdef LS3A4000
	if (NodeId) {
		//Config CPU1-HT1
		//Node 1
		//open RX window: 0x1000,0000,0000 ~ 0x1040,0000,0000
		Writel(Base + LS3A_HT_RX_CACHE_WIN2_OFFSET + 4, 0x4000c000);
		Writel(Base + LS3A_HT_RX_CACHE_WIN2_OFFSET, 0xc0100000);
		//Node 3
		//open RX window: 0x3000,0000,0000 ~ 0x3020,0000,0000
		Writel(Base + LS3A_HT_RX_CACHE_WIN3_OFFSET + 4, 0xc000e000);
		Writel(Base + LS3A_HT_RX_CACHE_WIN3_OFFSET, 0xc0300000);
		//open RX window: 0x3020,0000,0000 ~ 0x3030,0000,0000
		Writel(Base + LS3A_HT_RX_CACHE_WIN4_OFFSET + 4, 0xe000f000);
		Writel(Base + LS3A_HT_RX_CACHE_WIN4_OFFSET, 0xc0302000);
	} else {
		//Config CPU0-HT1
		//Node 2
		//open RX window: 0x2000,0000,0000 ~ 0x2040,0000,0000
		Writel(Base + LS3A_HT_RX_CACHE_WIN3_OFFSET + 4, 0x8000c000);
		Writel(Base + LS3A_HT_RX_CACHE_WIN3_OFFSET, 0xc0200000);
		//Node 1,for not 2 way connect
		//open RX window: 0x1000,0000,0000 ~ 0x1040,0000,0000
		Writel(Base + LS3A_HT_RX_CACHE_WIN4_OFFSET + 4, 0x4000c000);
		Writel(Base + LS3A_HT_RX_CACHE_WIN4_OFFSET, 0xc0100000);
	}
#else
	Writel(Base + LS3A_HT_RX_CACHE_WIN3_OFFSET + 4, 0x4000c000);
	Writel(Base + LS3A_HT_RX_CACHE_WIN3_OFFSET, 0xc0100000);
#endif
#endif

#if 0  //uncache DMA currently, configured by kernel
	//translate 0x80000000 ~ 0x90000000 to 0x0 ~ 0x10000000 for legacy kernel
	Writel(Base + LS3A_HT_RX_UNCACHE_WIN0_OFFSET + 4, 0x0080fff0);
	Writel(Base + LS3A_HT_RX_UNCACHE_WIN0_OFFSET, 0xc0000000);

	Writel(Base + LS3A_HT_RX_UNCACHE_WIN1_OFFSET + 4, 0x0000e000);
	Writel(Base + LS3A_HT_RX_UNCACHE_WIN1_OFFSET, 0x80000000);

#ifdef MULTI_CHIP
	//recover node 1 access address (bit 37: node id)
	Writel(Base + LS3A_HT_RX_UNCACHE_WIN2_OFFSET + 4, 0x2000e000);
	Writel(Base + LS3A_HT_RX_UNCACHE_WIN2_OFFSET, 0xc0100000);
#endif
#endif

	//set csr_dw_write to 1'b0 to transfer write mask infomation when write data less than 32Byte
	ls7a_debug("Disable 3A HT dw_write.\n");
#ifdef LS3A4000
	Val32 = Readl(Base + 0x1c0) & 0xfbffffff;
	Writel(Base + 0x1c0, Val32);
#else
	Val32 = Readl(Base + 0x50) & 0xff7fffff;
	Writel(Base + 0x50, Val32);
#endif

#ifdef LS3A4000
	ls7a_debug("Set 3A4000 HT int, using 8 bit int.\n");
	Val32 = Readl(Base + 0x1c4) | 0x00000400;
	Writel(Base + 0x1c4, Val32);
#endif

#ifdef LS3A4000
#ifdef ALLOW_EXT_IOI
	ls7a_debug("Set 3A4000 HT EXT int allow, allow kernel to use EXT_IOI.\n");
	Val32 = Readl(Base + 0x274) | 0x40000000;
	Writel(Base + 0x274, Val32);
#else
	ls7a_debug("NOT set 3A4000 HT EXT int allow, forbid kernel to use EXT_IOI.\n");
	Val32 = Readl(u64(0x900000001fe00000) + 0x8) & 0xfffffff7;
	Writel(u64(0x900000001fe00000) + 0x8, Val32);
#endif
#endif
	return 0;
}

int Ht7aSideConfigure(u64 Base)
{
	u32 Tmp;

	ls7a_debug("Open 7A HT RX space\n");
	//window0 00_0000_0000-7f_ffff_ffff
	Writel(Base + LS7A_HT_RX_WIN0_OFFSET + 4, 0);
	Writel(Base + LS7A_HT_RX_WIN0_OFFSET + 4, 0x00008000);
	Writel(Base + LS7A_HT_RX_WIN0_OFFSET, 0);
	Writel(Base + LS7A_HT_RX_WIN0_OFFSET, 0x80000000);
	ls7a_debug("Open 7A HT RX win0 space\n");

	//window1 fd_fc00_0000-fd_fcff_ffff
	Writel(Base + LS7A_HT_RX_WIN1_OFFSET + 4, 0xfdfcffff);
	Writel(Base + LS7A_HT_RX_WIN1_OFFSET, 0x80000000);
	ls7a_debug("Open 7A HT RX win1 space\n");

#if 0  //enable DMA post write
	if (LS7A_VERSION) {
		ls7a_debug("Enable 7A HT Post space\n");
		//window0 00_0000_0000-7f_ffff_ffff
		Writel(Base + LS7A_HT_TX_POST_WIN0_OFFSET + 4, 0x00008000);
		Writel(Base + LS7A_HT_TX_POST_WIN0_OFFSET, 0x80000000);
		Writel(Base + 0x1c4, Readl(Base + 0x1c4) & ~(1 << 11));  //disable wait
	}
#endif
	//set csr_dw_write to 1'b0 to transfer write mask infomation when write data less than 32Byte
	ls7a_debug("Disable 7A HT dw_write.\n");
	Tmp = Readl(Base + 0x1c0);
	Tmp &= 0xfbffffff;
	Writel(Base + 0x1c0, Tmp);

	//enable HT configure access
	ls7a_debug("Enable 7A HT configure access\n");
	Writel(Base + 0x168, 0xc000fe00);

	return 0;
}

int Ls3a7aHtConfigure (int NodeNum)
{
	u64 i;
	u64 Ht3aSideBase = HT1_CONTROLLER_CONF_BASE_ADDR;
	u64 Ht7aSideBase = HT_CONF_TYPE0_ADDR;

#ifdef MULTI_CHIP
#ifdef QUAD_CPU
	NodeNum = 2;
#endif
#ifndef LS7A_2WAY_CONNECT
	NodeNum = NodeNum - 1;
#endif
#endif
	for (i = 0; i < NodeNum; i++) {
		Ht3aSideBase |= (i << NODE_OFFSET);
		Ht7aSideBase |= (i << NODE_OFFSET);
#ifdef PRINT_HT1_REG
		u64 RegEnd, Reg;
		ls7a_debug("LS3A Node %lld HT registers before init\n", i);
		for (Reg = Ht3aSideBase, RegEnd = Reg + 0x180; Reg < RegEnd; Reg += 0x4) {
			ls7a_debug("0x%llx: 0x%016x\n", Reg, Readl(Reg));
		}

		ls7a_debug("LS7A to Node %lld HT registers before init\n", i);
		for (Reg = Ht7aSideBase, RegEnd = Reg + 0x268; Reg < RegEnd; Reg += 0x4) {
			ls7a_debug("0x%llx: 0x%016x\n", Reg, Readl(Reg));
		}
#endif

		//3A side HT init
		Ht3aSideConfigure(Ht3aSideBase, i);

		//7A side HT configure begin
		Ht7aSideConfigure(Ht7aSideBase);
#ifdef PRINT_HT1_REG
		ls7a_debug("LS3A Node %lld HT Registers after init\n", i);
		for (Reg = Ht3aSideBase, RegEnd = Reg + 0x180; Reg < RegEnd; Reg += 0x4) {
			ls7a_debug("0x%llx: 0x%016x\n", Reg, Readl(Reg));
		}

		ls7a_debug("LS7A to Node %lld HT Registers after init\n", i);
		for (Reg = Ht7aSideBase, RegEnd = Reg + 0x268; Reg < RegEnd; Reg += 0x4) {
			ls7a_debug("0x%llx: 0x%016x\n", Reg, Readl(Reg));
		}
#endif
  	}
	return 0;
}

int ConfigOnePll(u64 PllBase, u32 PllVal, u32 DivRefc)
{
	u32 i, Val32;

	Val32 = Readl(PllBase + 0x4) & ~(0x7 << LS7A_PLL_SEL0_OFFSET);//switch to backup clk
	Writel(PllBase + 0x4, Val32);
	Val32 = Readl(PllBase + 0x4) | (1 << LS7A_PLL_PD_OFFSET);//power down pll
	Writel(PllBase + 0x4, Val32);
	Val32 = Readl(PllBase + 0x4) & ~(1 << LS7A_PLL_SET_OFFSET);//disable pll configure
	Writel(PllBase + 0x4, Val32);

	//configure pll parameters
	Writel(PllBase, PllVal);
	Val32 = Readl(PllBase + 0x4);
	Val32 = (Val32 & ~(0x3f << LS7A_PLL_DIV_REFC_OFFSET)) | DivRefc;
	Writel(PllBase + 0x4, Val32);

	Val32 = Readl(PllBase + 0x4) | (1 << LS7A_PLL_SET_OFFSET);//enable pll configure
	Writel(PllBase + 0x4, Val32);
	Val32 = Readl(PllBase + 0x4) & ~(0x1 << LS7A_PLL_BYPASS_OFFSET);//not bypass pll
	Writel(PllBase + 0x4, Val32);
	Val32 = Readl(PllBase + 0x4) & ~(0x1 << LS7A_PLL_PD_OFFSET);//power up pll
	Writel(PllBase + 0x4, Val32);


	//poll lock signal
	for (i = 0; i < 0x1000; i++) {
		if (Readl(PllBase + 0x4) & (0x1 << LS7A_PLL_LOCK_OFFSET)) {
			//select pll out
			Val32 = Readl(PllBase + 0x4) | (0x7 << LS7A_PLL_SEL0_OFFSET);
			Writel(PllBase + 0x4, Val32);
			break;
		}
	}
	if (i == 0x1000)
		return -1;

	return 0;
}

int Ls7aPhyCfgWrite(u64 CfgAddr, u32 Data)
{
	do {
		if (Readl(CfgAddr + 0x4) & 0x4)
			break;
//		udelay(1);
	} while(1);

	Writel(CfgAddr, Data);
	Writel(CfgAddr + 0x4, 0x1);

	return 0;
}

u32 Ls7aPhyCfgRead(u64 CfgAddr, u32 Data)
{
	do {
		if (Readl(CfgAddr + 0x4) & 0x4)
			break;
//		udelay(1);
	} while(1);

	Writel(CfgAddr, Data);
	Writel(CfgAddr + 0x4, 0x0);

	do {
		if (Readl(CfgAddr + 0x4) & 0x4)
			break;
//		udelay(1);
	} while(1);

	return Readl(CfgAddr);
}

int DeviceCfg(Ls7aPcieInfo *PcieInfo)
{
	u32 Val32, Data = 0, Val32Bak;
	u16 CompRes;
	u64 i = 0;
	s32 WaitTime = 0;
	unsigned int OldScore;
	CurrentScore Result;
	unsigned int RightShift, AdjustValue;
	u64 ConfregAddr = 0x5cc + LS7A_CONFBUS_BASE_ADDR;

	unsigned int       EnSiTestFlag = 0;
	unsigned int       ClockFlag;
	unsigned int       ModeFlag;
//	unsigned int       TempData;
	unsigned int       ResetCount;

	int SATA0Enable = 1;
	int SATA1Enable = 1;
	int SATA2Enable = 1;
	int USB0Enable = 1;
	int USB1Enable = 1;
	int GMAC0Enable = 1;
	int GMAC1Enable = 1;

	int Ls7AVersion =  (LS7A_VERSION != 0);

	//
	// Locate mVariablePpi
	//

	//init PCIE -- v0.3
	//configure phy parameter
	for (i = 0; i < 0xa0; i += 0x20) {
		Writel(LS7A_CONFBUS_BASE_ADDR + 0x580 + i, 0xc2492331);
	}
	for (i = 0; i < 0xa0; i += 0x20) {
		Writel(LS7A_CONFBUS_BASE_ADDR + 0x584 + i, 0xff3ff0a8);
		//Writel(LS7A_CONFBUS_BASE_ADDR + 0x584 + i, 0x73e70b0);
	}
	for (i = 0; i < 0xa0; i += 0x20) {
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET + i, 0x27fff);
	}
	//assert reset
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) | ((1 << 28) | (1 << 24) | (1 << 20) | (1 << 16) | (1 << 8));
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET, Val32);

	//enable all ports
	for (i = 0; i < 0x40; i += 0x20) {
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET + i) | (0x1 << 27);
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET + i, Val32);
	}
	for (i = 0x40; i < 0xa0; i += 0x20) {
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET + i) | (0x3 << 26);
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET + i, Val32);
	}
	//power down phy
	for (i = 0; i < 0xa0; i += 0x20) {
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET + i) | (0x1 << 24);
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET + i, Val32);
	}
	//switch clk
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) | (1 << 2);
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET, Val32);
	udelay(100);
	//power up phy
	for (i = 0; i < 0xa0; i += 0x20) {
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET + i) & ~(0x1 << 24);
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET + i, Val32);
	}
	//deassert reset
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) & ~((1 << 28) | (1 << 24) | (1 << 20) | (1 << 16) | (1 << 8));
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET, Val32);
	//make sure all ports clock are ready
	do {
		if (((Readl(LS7A_CONFBUS_BASE_ADDR + 0x424) >> 8) & 0xfff) == 0xfff)
			break;
	} while(1);
	/*
	* note: this delay not enough will caused pcie controller status not stable
	* and some device con't be founded such as 82580 network in 8632 bridge
	*/
	udelay(1000);

	//For SI Test
	if (EnSiTestFlag) {
		ls7a_debug("== Start Software Configuration Mode ==\n");
		ModeFlag = EFI_LS7A_PCIE_X1_MODE;
//		LS7A_PCIE_GET_F0_MODE_VARIABLE();
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET) & ~(0x1 << 26);
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET, Val32);
		if (ModeFlag == EFI_LS7A_PCIE_X4_MODE) {
			Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET) | (0x3 << 26);
			Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET, Val32);
		} else {
			Data |= (0xe << 8);
		}

		ModeFlag = EFI_LS7A_PCIE_X1_MODE;
//		LS7A_PCIE_GET_F1_MODE_VARIABLE();
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F1_REG_OFFSET) & ~(0x1 << 26);
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F1_REG_OFFSET, Val32);
		if (ModeFlag == EFI_LS7A_PCIE_X4_MODE) {
			Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F1_REG_OFFSET) | (0x3 << 26);
			Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F1_REG_OFFSET, Val32);
		} else {
			Data |= (1 << 13);
		}

		ModeFlag = EFI_LS7A_PCIE_X8_MODE;
//		LS7A_PCIE_GET_H_MODE_VARIABLE();
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_H_REG_OFFSET) & ~(0x1 << 26);
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_H_REG_OFFSET, Val32);
		if (ModeFlag == EFI_LS7A_PCIE_X4_MODE) {
			Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_H_REG_OFFSET) | (0x3 << 26);
			Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_H_REG_OFFSET, Val32);
			Data |= (1 << 15);
		}

		ModeFlag = EFI_LS7A_PCIE_X8_MODE;
//		LS7A_PCIE_GET_G0_MODE_VARIABLE();
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G0_REG_OFFSET) & ~(0x1 << 26);
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G0_REG_OFFSET, Val32);
		if (ModeFlag == EFI_LS7A_PCIE_X4_MODE) {
			Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G0_REG_OFFSET) | (0x3 << 26);
			Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G0_REG_OFFSET, Val32);
			Data |= (1 << 17);
		}

		ModeFlag = EFI_LS7A_PCIE_X8_MODE;
//		LS7A_PCIE_GET_G1_MODE_VARIABLE();
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G1_REG_OFFSET) & ~(0x1 << 26);
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G1_REG_OFFSET, Val32);
		if (ModeFlag == EFI_LS7A_PCIE_X4_MODE) {
			Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G1_REG_OFFSET) | (0x3 << 26);
			Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G1_REG_OFFSET, Val32);
			Data |= (1 << 19);
		}
	} else {
//unless specified, recover to use prsnt pin to decide device existence.
#ifndef FORCE_ENABLE_PCIE_F0_P123
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET) & ~((0x1 << 27) | (1 << 26));
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET, Val32);
#else
		Data |= (0xe << 8);
#endif
#ifndef FORCE_ENABLE_PCIE_F1_P1
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F1_REG_OFFSET) & ~((0x1 << 27) | (1 << 26));
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F1_REG_OFFSET, Val32);
#else
		Data |= (1 << 13);
#endif
#ifndef FORCE_ENABLE_PCIE_H_P1
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_H_REG_OFFSET) & ~((0x1 << 27) | (1 << 26));
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_H_REG_OFFSET, Val32);
#else
		Data |= (1 << 15);
#endif
#ifndef FORCE_ENABLE_PCIE_G0_P1
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G0_REG_OFFSET) & ~((0x1 << 27) | (1 << 26));
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G0_REG_OFFSET, Val32);
#else
		Data |= (1 << 17);
#endif
#ifndef FORCE_ENABLE_PCIE_G1_P1
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G1_REG_OFFSET) & ~((0x1 << 27) | (1 << 26));
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G1_REG_OFFSET, Val32);
#else
		Data |= (1 << 19);
#endif
	}
	//assert reset
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) | ((1 << 28) | (1 << 24) | (1 << 20) | (1 << 16) | (1 << 8));
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET, Val32);
#ifdef USE_PCIE_PAD_REFCLK
	//power down phy
	for (i = 0; i < 0xa0; i += 0x20) {
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET + i) | (0x1 << 24);
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET + i, Val32);
	}
	//switch to use outside ref clk
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) & ~(1 << 2);
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET, Val32);
	udelay(100);
	//power up phy
	for (i = 0; i < 0xa0; i += 0x20) {
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET + i) & ~(0x1 << 24);
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET + i, Val32);
	}
#else
	udelay(100);
#endif
	//deassert reset
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) & ~((1 << 28) | (1 << 24) | (1 << 20) | (1 << 16) | (1 << 8));
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET, Val32);
	Data |= ((1 << 18) | (1 << 16) | (1 << 14) | (1 << 12) | (1 << 8));
	do {
		if ((Readl(LS7A_CONFBUS_BASE_ADDR + 0x424) & Data) == Data)
			break;
	} while(1);

#ifndef DISABLE_PCIEX8_CAL
	//for PCIE_H/G0/G1
	//
	// RightShift:t7: adjust dir(dll delay offset, 0: add delay at dll 0; 8: add deley at dll 1)
	// OldScore:t8: old score
	//

	do {
		Ls7aGetPcieDllScore(ConfregAddr, &Result);
		//find max of value 3 and c and determine the calibration direction
		//a0:AdjustValue;
		//when the two number has remarkable difference, start dll calibration
		if (Abs(Result.Clk1,Result.Clk0) >= (PCIE_PD_LOOP / 2)) {
			RightShift = 0;
			if (Result.Clk1 > Result.Clk0)
				RightShift = 8;

			do {
				OldScore = Result.CurrentScore;
				AdjustValue = ((Readl(ConfregAddr) >> RightShift) & 0xff);
				if (AdjustValue == 0xff)
					break;
				AdjustValue = (((AdjustValue << 1 ) | 0x1 ) & 0xff) << RightShift;
				Writel(ConfregAddr, AdjustValue);
				Ls7aGetPcieDllScore(ConfregAddr, &Result);

				//if this calibration has not make it remarkable worse, continue
			} while((Result.CurrentScore - OldScore) > -2);
			//make it worse a lot, scroll back
			Writel(ConfregAddr, (((Readl(ConfregAddr) >> RightShift ) & 0xff ) >> 1 ) << RightShift);
		}
		ConfregAddr += 0x20;
	} while(ConfregAddr <= (0x60c + LS7A_CONFBUS_BASE_ADDR));

#endif

	//use t0 as global variable
	//configure 7A sata PHY parameter
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SATA0_REG_OFFSET+0x4, 0x30c31cf9);
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SATA1_REG_OFFSET+0x4, 0x30c31cf9);
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SATA2_REG_OFFSET+0x4, 0x30c31cf9);

	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SATA0_REG_OFFSET, 0xf3000403);
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SATA1_REG_OFFSET, 0xf3000403);
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SATA2_REG_OFFSET, 0xf3000403);

//	LS7A_DEVICE_DISABLE_CLOCK_VARIABLE(SATA0,SATA0Enable);
	if (SATA0Enable) {
		#ifndef USE_SATA_PAD_REFCLK
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA0_REG_OFFSET+0x4) | (1 << 31);//power down phy
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SATA0_REG_OFFSET+0x4, Val32);
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA0_REG_OFFSET) | (1 << 2);//assert phy reset
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SATA0_REG_OFFSET, Val32);
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA0_REG_OFFSET) & ~(1 << 1);//switch refclk
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SATA0_REG_OFFSET, Val32);
		udelay(100);
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA0_REG_OFFSET+0x4) & ~(1 << 31); //power up phy
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SATA0_REG_OFFSET+0x4, Val32);
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA0_REG_OFFSET) & ~(1 << 2); //deassert phy reset
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SATA0_REG_OFFSET, Val32);
		udelay(100);
		#endif
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4) & ~(1 << 8);//deassert cntl reset
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4, Val32);
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4) | (1 << 10);//sata en
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4, Val32);
		#if OVRD_SATA_PHY
		Ls7aPhyCfgWrite(LS7A_CONFBUS_BASE_ADDR + CONF_SATA0_PHY_OFFSET, 0x403f1002);//configure phy
		#endif
		ls7a_debug("SATA0 enabled\r\n");
	} else {
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA0_REG_OFFSET + 0x4) | (1 << 31); //powerdown phy
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SATA0_REG_OFFSET + 0x4, Val32);
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4) & ~(1 << 11);//deassert cntl reset
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4, Val32);
		ls7a_debug("SATA0 clock disabled\r\n");
	}

//	LS7A_DEVICE_DISABLE_CLOCK_VARIABLE(SATA1,SATA1Enable);
	if (SATA1Enable) {
		#ifndef USE_SATA_PAD_REFCLK
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA1_REG_OFFSET + 0x4) | (1 << 31);//power down phy
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SATA1_REG_OFFSET + 0x4, Val32);
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA1_REG_OFFSET) | (1 << 2);//assert phy reset
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SATA1_REG_OFFSET, Val32);
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA1_REG_OFFSET) & ~(1 << 1);//switch refclk
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SATA1_REG_OFFSET, Val32);
		udelay(100);
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA1_REG_OFFSET + 0x4) & ~(1 << 31); //power up phy
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SATA1_REG_OFFSET + 0x4, Val32);
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA1_REG_OFFSET) & ~(1 << 2); //deassert phy reset
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SATA1_REG_OFFSET, Val32);
		udelay(100);
		#endif
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4) & ~(1 << 12);//deassert cntl reset
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4, Val32);
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4) | (1 << 14);//sata en
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4, Val32);
		#if OVRD_SATA_PHY
		Ls7aPhyCfgWrite(LS7A_CONFBUS_BASE_ADDR + CONF_SATA1_PHY_OFFSET, 0x403f1002);//configure phy
		#endif
		ls7a_debug("SATA1 enabled\r\n");
	} else {
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA1_REG_OFFSET + 0x4) | (1 << 31);
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SATA1_REG_OFFSET + 0x4, Val32);
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4) & ~(1 << 15);//deassert cntl reset
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4, Val32);
		ls7a_debug("SATA1 clock disabled\r\n");
	}

//	LS7A_DEVICE_DISABLE_CLOCK_VARIABLE(SATA2,SATA2Enable);
	if (SATA2Enable) {
		#ifndef USE_SATA_PAD_REFCLK
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA2_REG_OFFSET + 0x4) | (1 << 31);//power down phy
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SATA2_REG_OFFSET + 0x4, Val32);
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA2_REG_OFFSET) | (1 << 2);//assert phy reset
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SATA2_REG_OFFSET, Val32);
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA2_REG_OFFSET) & ~(1 << 1);//switch refclk
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SATA2_REG_OFFSET, Val32);
		udelay(100);
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA2_REG_OFFSET + 0x4) & ~(1 << 31); //power up phy
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SATA2_REG_OFFSET + 0x4, Val32);
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA2_REG_OFFSET) & ~(1 << 2); //deassert phy reset
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SATA2_REG_OFFSET, Val32);
		udelay(100);
		#endif
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4) & ~(1 << 16);//deassert cntl reset
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4, Val32);
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4) | (1 << 18);//sata en
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4, Val32);
		#if OVRD_SATA_PHY
		Ls7aPhyCfgWrite(LS7A_CONFBUS_BASE_ADDR + CONF_SATA2_PHY_OFFSET, 0x403f1002);//configure phy
		#endif
		ls7a_debug("SATA2 enabled\r\n");
	} else {
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA2_REG_OFFSET + 0x4) | (1 << 31);
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SATA2_REG_OFFSET + 0x4, Val32);
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4) & ~(1 << 19);//deassert cntl reset
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4, Val32);
		ls7a_debug("SATA2 clock disabled\r\n");
	}

	// init USB
#ifdef  USE_USB_SYS_REFCLK
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) | (0x2 << 2);//switch refclk
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET, Val32);
	udelay(100);
#endif

//	LS7A_DEVICE_DISABLE_CLOCK_VARIABLE(USB0,USB0Enable);
	if (USB0Enable) {
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) & ~(0x1 << 9);//deassert phy reset
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET, Val32);
		udelay(100);
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) & ~(0x1 << 8);//deassert cntl reset
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET, Val32);
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) | (0x1 << 10);
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET, Val32);
		ls7a_debug("USB0 enabled\r\n");
	} else {
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) & ~(0x1 << 11);//disable clock
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET, Val32);
		ls7a_debug("USB0 clock disabled\r\n");
	}

//	LS7A_DEVICE_DISABLE_CLOCK_VARIABLE(USB1,USB1Enable);
	if (USB1Enable) {
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) & ~(0x1 << 13);//deassert phy reset
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET, Val32);
		udelay(100);
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) & ~(0x1 << 12);//deassert cntl reset
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET, Val32);
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) | (0x1 << 14);
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET, Val32);
		ls7a_debug("USB1 enabled\r\n");
	} else {
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) & ~(0x1 << 15);//disable clock
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET, Val32);
		ls7a_debug("USB1 clock disabled\r\n");
	}

//	LS7A_DEVICE_DISABLE_CLOCK_VARIABLE(GMAC0,GMAC0Enable);
	if (!GMAC0Enable) {
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) & ~(0x1 << 5);
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET, Val32);
		ls7a_debug("GMAC0 disabled.\r\n");
	}

//	LS7A_DEVICE_DISABLE_CLOCK_VARIABLE(GMAC1,GMAC1Enable);
	if (!GMAC1Enable) {
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) & ~(0x1 << 7);
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET, Val32);
		ls7a_debug("GMAC1 disabled.\r\n");
	}

// GMEM configure -- put to Device initial code ?
#if (!LS7A_LPC_DISABLE)
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4) | (0x1 << 0);
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4, Val32);
	ls7a_debug("LPC enabled\r\n");
#endif

#if (LS7A_GRAPHICS_DISABLE)
	//TODO, can not disable DC because the PMON assume there it is and as the GPU BAR will affect DC BAR's found, so GPU cannot be disabled also
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) & ~(0x1 << 5);
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET, Val32);
	ls7a_debug("Graphics disabled.\r\n");
#endif

#define PCIE_TRY_GEN2 1
#define PCIE_TX_FULL_SWING 0
	udelay(10000); // delay at least 100ms?
	//PCIE F0
	//re-configure PCIE PHYs
	//DO not destroy a0, a1, for example, do not add print between these code
#if (!LS7A_PCIE_F0_DISABLE)
	//enable access
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) | (1 << 9);
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET, Val32);
	ls7a_debug("PCIE F0 enabled\r\n");
	Data = 0;
	for (i = 0; i < 0x400; i += 0x100) {
		Ls7aPhyCfgWrite(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_PHY_OFFSET, 0x4fff1002 + i);
	}

	LS7A_PCIE_CFG(0x4800, 0x60000000, 0x1, F0_P0_End)
	if (Ls7AVersion) {
		Val32 = LS7A_PCIE_CFG_READ(0x60000000,0x28);
		LS7A_PCIE_CFG_WRITE(0x60000000,0x28,Val32 | 0x1);
	}
	//read x4_mode to decide whether P1~P3 is valid
	if(((LS7A_PCIE_CFG_READ(0x60000000,0x28) >> 26) & 1))
		Data |= 0xe;
	LS7A_PCIE_CFG_DONE(0x4800);
	PcieInfo->PcieCfgBuffer[PcieInfo->Num++] = 0x4800;

	if (!(Data > 0x1)) {
		//Port 1
		LS7A_PCIE_CFG(0x5000, 0x60100000, 0x2, F0_P1_End)
		if (Ls7AVersion) {
			Val32 = LS7A_PCIE_CFG_READ(0x60100000,0x28);
			LS7A_PCIE_CFG_WRITE(0x60100000,0x28,Val32 | 0x1);
		}
		LS7A_PCIE_CFG_DONE(0x5000);
		PcieInfo->PcieCfgBuffer[PcieInfo->Num++] = 0x5000;

		//Port 2
		LS7A_PCIE_CFG(0x5800, 0x60200000, 0x4, F0_P2_End)
		if (Ls7AVersion) {
			Val32 = LS7A_PCIE_CFG_READ(0x60200000,0x28);
			LS7A_PCIE_CFG_WRITE(0x60200000,0x28,Val32 | 0x1);
		}
		LS7A_PCIE_CFG_DONE(0x5800);
		PcieInfo->PcieCfgBuffer[PcieInfo->Num++] = 0x5800;

		//Port 3
		LS7A_PCIE_CFG(0x6000, 0x60300000, 0x8, F0_P3_End)
		if (Ls7AVersion) {
			Val32 = LS7A_PCIE_CFG_READ(0x60300000,0x28);
			LS7A_PCIE_CFG_WRITE(0x60300000,0x28,Val32 | 0x1);
		}
		LS7A_PCIE_CFG_DONE(0x6000);
		PcieInfo->PcieCfgBuffer[PcieInfo->Num++] = 0x6000;
	}
#ifdef  LS7A_PCIE_NO_POWERDOWN
	Data = 0;
#endif
#else  //disable this PCIE
	Data = 0xf;
#endif

	ClockFlag = CLOCK_AS_DEFAULT;
	if (EnSiTestFlag) {
//		LS7A_PCIE_GET_F0_CLOCK_VARIABLE();
	}
	if (!ClockFlag) {
		if(Data ==  0xf){
			Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET) | (0x1 << 24);
			Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET, Val32);
			Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) & (~(1 << 9));
			Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET, Val32);
			ls7a_debug("Powerdown PCIE F0 PHY and disable all Ports\n");
		}

		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) & (~(Data << 10));
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET, Val32);
		ls7a_debug("unused PCIE F0 ports clock disabled,%x\n",(~(Data << 10)));
	}

	//PCIE F1
	//some port is used
	//re-configure PHYs
#if (!LS7A_PCIE_F1_DISABLE)
	//enable access
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) | (1 << 17);
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET, Val32);
	ls7a_debug("PCIE F1 enabled\r\n");
	Data = 0;
	for (i = 0; i < 0x400; i += 0x100) {
		Ls7aPhyCfgWrite(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F1_PHY_OFFSET, 0x4fff1002 + i);
	}

	LS7A_PCIE_CFG(0x6800, 0x60000000, 0x1, F1_P0_End)
	if (Ls7AVersion) {
		Val32 = LS7A_PCIE_CFG_READ(0x60000000,0x28);
		LS7A_PCIE_CFG_WRITE(0x60000000,0x28,Val32 | 0x1);
	}
	//read x4_mode to decide whether P1 is valid
	if (((LS7A_PCIE_CFG_READ(0x60000000,0x28) >> 26) & 1))
		//x4 mode
		Data |= 0x2;
	LS7A_PCIE_CFG_DONE(0x6800);
	PcieInfo->PcieCfgBuffer[PcieInfo->Num++] = 0x6800;

	if (!(Data >  0x1)) {
		//Port 1
		LS7A_PCIE_CFG(0x7000, 0x60100000, 0x2, F1_P1_End)
		if (Ls7AVersion) {
			Val32 = LS7A_PCIE_CFG_READ(0x60100000,0x28);
			LS7A_PCIE_CFG_WRITE(0x60100000,0x28,Val32 | 0x1);
		}
		LS7A_PCIE_CFG_DONE(0x7000);
		PcieInfo->PcieCfgBuffer[PcieInfo->Num++] = 0x7000;
	}
#ifdef  LS7A_PCIE_NO_POWERDOWN
	Data = 0;
#endif
#else  //disable this PCIE
	Data = 0x3;
#endif
	ClockFlag = CLOCK_AS_DEFAULT;
	if (EnSiTestFlag) {
//		LS7A_PCIE_GET_F1_CLOCK_VARIABLE();
	}
	if (!ClockFlag) {
		if (Data ==  0x3) {
			Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F1_REG_OFFSET) | (0x1 << 24);
			Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F1_REG_OFFSET, Val32);
			Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) & (~(1 << 17));
			Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET, Val32);
			ls7a_debug("Powerdown PCIE F1 PHY and disable all Ports\n");
		}
		//disable clock of unused PCIE ports
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) & (~(Data << 18));
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET, Val32);
		ls7a_debug("unused PCIE F1 ports clock disabled %x\n",(~(Data << 18)));
	}

	//PCIE H
	//some port is used
	//re-configure PHYs
#if (!LS7A_PCIE_H_DISABLE)
	//enable access
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) | (1 << 21);
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET, Val32);
	ls7a_debug("PCIE H enabled\r\n");

	for (ResetCount = 0; ResetCount <= 7; ResetCount++) {
		Data = 0;
		for (i = 0; i < 0x400; i += 0x100) {
			Ls7aPhyCfgWrite(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_H_PHY_LO_OFFSET, 0x4fff1002 + i);
		}
		for (i = 0; i < 0x400; i += 0x100) {
			Ls7aPhyCfgWrite(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_H_PHY_HI_OFFSET, 0x4fff1002 + i);
		}
		LS7A_PCIE_CFG(0x9800, 0x60000000, 0x1, H_P0_End)
		if (Ls7AVersion) {
			Val32 = LS7A_PCIE_CFG_READ(0x60000000,0x28);
			LS7A_PCIE_CFG_WRITE(0x60000000,0x28,Val32 | 0x1);
		}
		Val32 = LS7A_PCIE_CFG_READ(0x60000000,0x0);

		if(((Val32 >> 28) != 0xf) && (ResetCount < 5)) {  /*present=0 & X8 Reset*/
			if (!((LS7A_PCIE_CFG_READ(0x60000000,0xc) & 0x40) == 0x0)) { /*link*/
				LS7A_PCIE_LINK_VAL_RESTORE(0x9800);
				CompRes = StatVsCap(0x9800, 0x5);
				if (CompRes == 0) {
					LS7A_PCIE_LINK_VAL_BACKUP(0x9800);
					break;
				}
			}
			/*Not Link & Compared fail*/
			LS7A_PCIE_RST_CTRL(1 << 20);
		} else if (((Val32 >> 28) != 0xf) && (ResetCount < 7)) {/*Start Enable X4 Reset*/
			if (((LS7A_PCIE_CFG_READ(0x60000000,0xc) & 0x40) == 0x0)) { /*not link*/
				Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_H_REG_OFFSET) | (0x3 << 26);
				Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_H_REG_OFFSET, Val32);
				LS7A_PCIE_RST_CTRL(1 << 20);
			} else {
				break;
			}
		}
	}

	//read x4_mode to decide whether P1 is valid
	if (!((LS7A_PCIE_CFG_READ(0x60000000,0x28) >> 26) & 1))
		//x8 mode
		Data |= 0x2;
	LS7A_PCIE_CFG_DONE(0x9800);
	PcieInfo->PcieCfgBuffer[PcieInfo->Num++] = 0x9800;

	if (!(Data > 0x1)) {
		//Port 1
		LS7A_PCIE_CFG(0xa000, 0x60100000, 0x2, H_P1_End)
		if (Ls7AVersion) {
			Val32 = LS7A_PCIE_CFG_READ(0x60100000,0x28);
			LS7A_PCIE_CFG_WRITE(0x60100000,0x28,Val32 | 0x1);
		}
		LS7A_PCIE_CFG_DONE(0xa000);
		PcieInfo->PcieCfgBuffer[PcieInfo->Num++] = 0xa000;
	}
#ifdef  LS7A_PCIE_NO_POWERDOWN
	Data = 0;
#endif
#else  //disable this PCIE
	Data = 0x3;
#endif
	ClockFlag = CLOCK_AS_DEFAULT;
	if (EnSiTestFlag) {
//		LS7A_PCIE_GET_H_CLOCK_VARIABLE();
	}
	if (!ClockFlag) {
		if (Data ==  0x3) {
			Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_H_REG_OFFSET) | (0x3 << 24);
			Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_H_REG_OFFSET, Val32);
			Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) & (~(1 << 21));
			Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET, Val32);
			ls7a_debug("Powerdown PCIE H PHY and disable all Ports\n");
		}
		//disable clock of unused PCIE ports
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) & (~(Data << 22));
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET, Val32);
		ls7a_debug("unused PCIE H ports clock disabled %x\n",(~(Data << 22)));
	}

	//PCIE G0
	//some port is used
	//re-configure PHYs
#if (!LS7A_PCIE_G0_DISABLE)
	//enable access
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) | (1 << 25);
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET, Val32);
	ls7a_debug("PCIE G0 enabled\r\n");

	for (ResetCount = 0; ResetCount <= 7; ResetCount++) {
		Data = 0;
		for (i = 0; i < 0x400; i += 0x100) {
			Ls7aPhyCfgWrite(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G0_PHY_LO_OFFSET, 0x4fff1002 + i);
		}
		for (i = 0; i < 0x400; i += 0x100) {
			Ls7aPhyCfgWrite(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G0_PHY_HI_OFFSET, 0x4fff1002 + i);
		}
		LS7A_PCIE_CFG(0x7800, 0x60000000, 0x1, G0_P0_End)
		if (Ls7AVersion) {
			Val32 = LS7A_PCIE_CFG_READ(0x60000000,0x28);
			LS7A_PCIE_CFG_WRITE(0x60000000,0x28,Val32 | 0x1);
		}
		Val32 = LS7A_PCIE_CFG_READ(0x60000000,0x0);

		if (((Val32 >> 28) != 0xf) && (ResetCount < 5)) {  /*present=0 & X8 Reset*/
			if (!((LS7A_PCIE_CFG_READ(0x60000000,0xc) & 0x40) == 0x0)) { /*link*/
				LS7A_PCIE_LINK_VAL_RESTORE(0x7800);
				CompRes = StatVsCap(0x7800, 0x5);
				if (CompRes == 0) {
					LS7A_PCIE_LINK_VAL_BACKUP(0x7800);
					break;
				}
			}
			/*Not Link & Compared fail*/
			LS7A_PCIE_RST_CTRL(1 << 24);
		} else if (((Val32 >> 28) != 0xf) && (ResetCount < 7)) {/*Start Enable X4 Reset*/
			if (((LS7A_PCIE_CFG_READ(0x60000000,0xc) & 0x40) == 0x0)) { /*not link*/
				Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G0_REG_OFFSET) | (0x3 << 26);
				Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G0_REG_OFFSET, Val32);
				LS7A_PCIE_RST_CTRL(1 << 24);
			} else {
				break;
			}
		}
	}

	//read x4_mode to decide whether P1 is valid
	if (!((LS7A_PCIE_CFG_READ(0x60000000,0x28) >> 26) & 1))
		Data |= 0x2;
	LS7A_PCIE_CFG_DONE(0x7800);
	PcieInfo->PcieCfgBuffer[PcieInfo->Num++] = 0x7800;

	if (!(Data > 0x1)) {
		//Port 1
		LS7A_PCIE_CFG(0x8000, 0x60100000, 0x2, G0_P1_End)
		if (Ls7AVersion) {
			Val32 = LS7A_PCIE_CFG_READ(0x60100000,0x28);
			LS7A_PCIE_CFG_WRITE(0x60100000,0x28,Val32 | 0x1);
		}
		LS7A_PCIE_CFG_DONE(0x8000);
		PcieInfo->PcieCfgBuffer[PcieInfo->Num++] = 0x8000;
	}
#ifdef  LS7A_PCIE_NO_POWERDOWN
	Data = 0;
#endif
#else  //disable this PCIE
	Data = 0x3;
#endif
	ClockFlag = CLOCK_AS_DEFAULT;
	if (EnSiTestFlag) {
//		LS7A_PCIE_GET_G0_CLOCK_VARIABLE();
	}
	if (!ClockFlag) {
		if (Data ==  0x3) {
			Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G0_REG_OFFSET) | (0x3 << 24);
			Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G0_REG_OFFSET, Val32);
			Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) & (~(1 << 25));
			Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET, Val32);
			ls7a_debug("Powerdown PCIE G0 PHY and disable all Ports\n");
		}
		//disable clock of unused PCIE ports
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) & (~(Data << 26));
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET, Val32);
		ls7a_debug("unused PCIE G0 port clock disabled %x\n",(~(Data << 26)));
	}

	//PCIE G1
	//some port is used
	//re-configure PHYs
#if (!LS7A_PCIE_G1_DISABLE)
	//enable access
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) | (1 << 29);
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET, Val32);
	ls7a_debug("PCIE G1 enabled\r\n");

	for (ResetCount = 0; ResetCount <= 7; ResetCount++) {
		Data = 0;
		for (i = 0; i < 0x400; i += 0x100) {
			Ls7aPhyCfgWrite(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G1_PHY_LO_OFFSET, 0x4fff1002 + i);
		}
		for (i = 0; i < 0x400; i += 0x100) {
			Ls7aPhyCfgWrite(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G1_PHY_HI_OFFSET, 0x4fff1002 + i);
		}
		LS7A_PCIE_CFG(0x8800, 0x60000000, 0x1, G1_P0_End)
		if (Ls7AVersion) {
			Val32 = LS7A_PCIE_CFG_READ(0x60000000,0x28);
			LS7A_PCIE_CFG_WRITE(0x60000000,0x28,Val32 | 0x1);
		}
		Val32 = LS7A_PCIE_CFG_READ(0x60000000,0x0);

		if (((Val32 >> 28) != 0xf) && (ResetCount < 5)) {  /*present=0 & X8 Reset*/
			if (!((LS7A_PCIE_CFG_READ(0x60000000,0xc) & 0x40) == 0x0)) { /*link*/
				LS7A_PCIE_LINK_VAL_RESTORE(0x8800);
				CompRes = StatVsCap(0x8800, 0x5);
				if (CompRes == 0) {
					LS7A_PCIE_LINK_VAL_BACKUP(0x8800);
					break;
				}
			}
			/*Not Link & Compared fail*/
			LS7A_PCIE_RST_CTRL(1 << 28);
		} else if (((Val32 >> 28) != 0xf) && (ResetCount < 7)) {/*Start Enable X4 Reset*/
			if (((LS7A_PCIE_CFG_READ(0x60000000,0xc) & 0x40) == 0x0)) { /*not link*/
				Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G1_REG_OFFSET) | (0x3 << 26);
				Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G1_REG_OFFSET, Val32);
				LS7A_PCIE_RST_CTRL(1 << 28);
			} else {
				break;
			}
		}
	}

	//read x4_mode to decide whether P1 is valid
	if (!((LS7A_PCIE_CFG_READ(0x60000000,0x28) >> 26) & 1))
		Data |= 0x2;
	LS7A_PCIE_CFG_DONE(0x8800);
	PcieInfo->PcieCfgBuffer[PcieInfo->Num++] = 0x8800;

	if (!(Data > 0x1)) {
		//Port 1
		LS7A_PCIE_CFG(0x9000, 0x60100000, 0x2, G1_P1_End)
		if (Ls7AVersion) {
			Val32 = LS7A_PCIE_CFG_READ(0x60100000,0x28);
			LS7A_PCIE_CFG_WRITE(0x60100000,0x28,Val32 | 0x1);
		}
		LS7A_PCIE_CFG_DONE(0x9000);
		PcieInfo->PcieCfgBuffer[PcieInfo->Num++] = 0x9000;
	}
#ifdef  LS7A_PCIE_NO_POWERDOWN
	Data = 0;
#endif
#else  //disable this PCIE
	Data = 0x3;
#endif
	ClockFlag = CLOCK_AS_DEFAULT;
	if (EnSiTestFlag) {
//		LS7A_PCIE_GET_G1_CLOCK_VARIABLE();
	}
	if (!ClockFlag) {
		if (Data ==  0x3) {
			Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G1_REG_OFFSET) | (0x3 << 24);
			Writel(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G1_REG_OFFSET, Val32);
			Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) & (~(1 << 29));
			Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET, Val32);
			ls7a_debug("Powerdown PCIE G1 PHY and disable all Ports\n");
		}
		//disable clock of unused PCIE ports
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) & (~(Data << 30));
		Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET, Val32);
		ls7a_debug("unused PCIE G1 ports clock disabled %x\n",(~(Data << 30)));
	}

#if LS7A_GMEM_CFG
	u8               GMemModuleType;
//	u64              *GmemParam = NULL;
//	PEI_GET_MEMPARAM    GetMemParamPei;
//	volatile u64     *pMcRegsBase = NULL;
	//
	//this code if should be add, is at: pmon/arch/mips/ls7a/ls7a_init.S
	//
	Writel(GPU_HEADER_ADDR + 0x18, TEMP_GMEM_ADDR);
	Writel(GPU_HEADER_ADDR + 0x1c, 0x0);
	Writel(GPU_HEADER_ADDR + 0x04, 0x2);

	ls7a_debug("Enable gmem config space\r\n");
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET + 0x4) & ~(0x1 << 24);
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET + 0x4, Val32);

	//
	// Locate PeiMemParamPpi
	//
/*	Status = PeiServicesLocatePpi (&gEfiPeiMemParamPpiGuid, 0, NULL, (void **) &GetMemParamPei);
	if (EFI_ERROR (Status))
		return EFI_UNSUPPORTED;

	GetMemParamPei(NULL, NULL, NULL, &GmemParam);*/

	for (i=0;i<MC_REGS_COUNT;i++) {
//		pMcRegsBase = (u64 *)(LS7A_GMEM_TEMP_ADDR + (i<<3));
//		*pMcRegsBase = GmemParam[i];
		Writeq(LS7A_GMEM_TEMP_ADDR + (i<<3), GmemParam[i]);
	}

	Val32 = Readb(LS7A_GMEM_TEMP_ADDR + 0x18) | 0x1;
	Writeb(LS7A_GMEM_TEMP_ADDR + 0x18, Val32);
//	Readb(LS7A_GMEM_TEMP_ADDR + 0x18) |= 0x1;
#if 0
	DebugPrint(EFI_D_INFO, "The GMEM parameter is:\r\n");
	for (i=0;i<MC_REGS_COUNT;i++) {
		pMcRegsBase = (u64 *)(LS7A_GMEM_TEMP_ADDR + (i<<3));
//		u64 data = *pMcRegsBase;
		u64 data = Readq(pMcRegsBase);
		DebugPrint(EFI_D_INFO, "%03x: %016lx\r\n", pMcRegsBase, data);
	}
#endif

	while (!Readb(LS7A_GMEM_TEMP_ADDR + 0x163)) {
		/* Just for delay */
		for (i=0; i<0x1000; i++);
	}

	GMemModuleType = 0x0;//RDIMM is 0x1; UDIMM is 0x2, here send 0x0 to leveling; otherwise, the 7A's GPU will worked unnormal.
	DDR3Leveling(LS7A_GMEM_TEMP_ADDR, 0x0, GMemModuleType, 2, 0x0);

#if 0
	DebugPrint(EFI_D_INFO, "The GMEM parameter is:\r\n");
	for (i=0;i<MC_REGS_COUNT;i++) {
		pMcRegsBase = (u64 *)(LS7A_GMEM_TEMP_ADDR + (i<<3));
//		u64 data = *pMcRegsBase;
		u64 data = Readq(pMcRegsBase);
		DebugPrint(EFI_D_INFO, "%03x: %016lx\r\n", pMcRegsBase, data);
	}
#endif

	ls7a_debug("Disable gmem config space\r\n");
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET + 0x4) | (0x1 << 24);
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET + 0x4, Val32);

	//set gmem space bar mask
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_GMEM_BAR_MASK_OFFSET, 0x07ffffff);
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_GMEM_BAR_MASK_OFFSET + 0x4, 0x0);


//	u64 GmemAddr = LS7A_GMEM_TEMP_ADDR;
	Writeq(LS7A_GMEM_TEMP_ADDR+0x00, 0x5555555555555555);
	Writeq(LS7A_GMEM_TEMP_ADDR+0x08, 0xaaaaaaaaaaaaaaaa);
	Writeq(LS7A_GMEM_TEMP_ADDR+0x10, 0x3333333333333333);
	Writeq(LS7A_GMEM_TEMP_ADDR+0x18, 0xcccccccccccccccc);
	Writeq(LS7A_GMEM_TEMP_ADDR+0x20, 0x7777777777777777);
	Writeq(LS7A_GMEM_TEMP_ADDR+0x28, 0x8888888888888888);
	Writeq(LS7A_GMEM_TEMP_ADDR+0x30, 0x1111111111111111);
	Writeq(LS7A_GMEM_TEMP_ADDR+0x38, 0xeeeeeeeeeeeeeeee);


	ls7a_debug("The gmem data is:\r\n");
	for (i=0; i < 8; i++) {
		ls7a_debug(" 0x%03llx : 0x%llx\r\n", i*8, Readq(LS7A_GMEM_TEMP_ADDR + i*8));
	}

	//recover gpu bar
	Writel(GPU_HEADER_ADDR + 0x04, 0);
	Writel(GPU_HEADER_ADDR + 0x18, 0);
	Writel(GPU_HEADER_ADDR + 0x1c, 0);

#endif

	return 0;
}

int EnableGmacWol(void)
{
	u32 Val32;
	//set GPE0_EN bit 7 to 1 enable USB_GMAC_OK resume
	Val32 = Readl(LS7A_MISC_BASE + LS7A_ACPI_OFFSET + 0x4) | (0x1 << 7);
	Writel(LS7A_MISC_BASE + LS7A_ACPI_OFFSET + 0x4, Val32);

	//set wol_en and wol_bat_en
	Val32 = Readl(LS7A_MISC_BASE + LS7A_ACPI_OFFSET + 0x8) | (0x1 << 7) | (0x1 << 8);
	Writel(LS7A_MISC_BASE + LS7A_ACPI_OFFSET + 0x8, Val32);

#ifdef OLD_BP
	//set GPE0_EN bit 5/6 to 1 ebale gmac0/gmac1 wol
	Val32 = Readl(LS7A_MISC_BASE + LS7A_ACPI_OFFSET + 0x2c) | (0x1 << 5) | (0x1 << 6);
	Writel(LS7A_MISC_BASE + LS7A_ACPI_OFFSET + 0x2c, Val32);
#endif

	ls7a_debug("EnableGmacWol Func\n");
	return 0;
}

int EnablePcieWake(void)
{
	u32 Val32;
	//clear pcie wake disable
	Val32 = Readl(LS7A_MISC_BASE + LS7A_ACPI_OFFSET + 0x10) & ~(0x1 << 14);
	Writel(LS7A_MISC_BASE + LS7A_ACPI_OFFSET + 0x10, Val32);

	ls7a_debug("EnablePcieWake Func\n");
	return 0;
}

int EnableUsbWake(void)
{
	u32 Val32;
	//set GPE0_EN bit 7 to 1 enable USB_GMAC_OK resume
	Val32 = Readl(LS7A_MISC_BASE + LS7A_ACPI_OFFSET + 0x4) | (0x1 << 7);
	Writel(LS7A_MISC_BASE + LS7A_ACPI_OFFSET + 0x4, Val32);

#ifdef OLD_BP
	//set key,usb wakeup of reg GPE0_EN
	Val32 = Readl(LS7A_MISC_BASE + LS7A_ACPI_OFFSET + 0x2c) | (0x1 << 8) | (0x3f<<10);
	Writel(LS7A_MISC_BASE + LS7A_ACPI_OFFSET + 0x2c, Val32);
#endif

	ls7a_debug("EnableUsbWake Func\n");
	return 0;
}

int EnableRtcWake(void)
{
	u32 Val32;
	//set wol_en and wol_bat_en
	Val32 = Readl(LS7A_MISC_BASE + LS7A_ACPI_OFFSET + 0x8) | (0x1 << 7) | (0x1 << 8);
	Writel(LS7A_MISC_BASE + LS7A_ACPI_OFFSET + 0x8, Val32);

	//enable rtc wake
	Val32 = Readl(LS7A_MISC_BASE + LS7A_ACPI_OFFSET + 0x10) | (0x1 << 10);
	Writel(LS7A_MISC_BASE + LS7A_ACPI_OFFSET + 0x10, Val32);

	ls7a_debug("EnableRtcWake Func\n");
	return 0;
}

void ClearAcpiReg(void)
{
	//clear acpi status register
	Writel(LS7A_ACPI_BASE + 0xc,  Readl(LS7A_ACPI_BASE + 0xc));
	Writel(LS7A_ACPI_BASE + 0x28, Readl(LS7A_ACPI_BASE + 0x28));
//	Writel(LS7A_ACPI_BASE + 0x2c, Readl(LS7A_ACPI_BASE + 0x2c));
	Writel(LS7A_ACPI_BASE + 0x2c, 0x0);

	Writel(LS7A_ACPI_BASE + 0x4,0);
	/* set vsb_gate_delay */
	Writel(LS7A_ACPI_BASE + 0x4, Readl(LS7A_ACPI_BASE + 0x4) | (0x5 << 11));
	Writel(LS7A_ACPI_BASE + 0x8, (Readl(LS7A_ACPI_BASE + 0x8) & (~0x1FE)) | 0x1);
	Writel(LS7A_ACPI_BASE + 0x10,0);
	Writel(LS7A_ACPI_BASE + 0x14,0);

	// disable pcie wake en
	Writel(LS7A_ACPI_BASE + 0x10, Readl(LS7A_ACPI_BASE + 0x10) | (0x1 << 14));
}

void ConfigLs7aWakeUpFunc(void)
{
	int WakeGmacEnable = 1;
	int WakePcieEnable = 1;
	int WakeUsbEnable = 1;
	int WakeRtcEnable = 1;

	ClearAcpiReg();

	// enable autoboot when power on form G3.
//	Writel(LS7A_ACPI_BASE + 0x8, Readl(LS7A_ACPI_BASE + 0x8) & ~0x1);

	if (WakeGmacEnable) {
		EnableGmacWol();
	}

	if (WakePcieEnable) {
		EnablePcieWake();
	}

	if (WakeUsbEnable) {
		EnableUsbWake();
	}

	if (WakeRtcEnable) {
		EnableRtcWake();
	}
}

int Ls7aResourceConfigure(Ls7aPcieInfo *PcieInfo)
{
	u64 i = 0;
	u32 Val32;

	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_HT_ROUTE_OFFSET) & ~(0xf | (0xf << 16));
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_HT_ROUTE_OFFSET, Val32);
#ifdef MULTI_CHIP
#ifdef LS7A_2WAY_CONNECT
	// support 2 Nodes
	// route Node0 DMA to 7A HT Lo
	// route Node1 DMA to 7A HT Hi
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_HT_ROUTE_OFFSET) | (0xa << 16);
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_HT_ROUTE_OFFSET, Val32);
#endif
#ifdef QUAD_CPU
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_HT_ROUTE_OFFSET) | 0x3;
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_HT_ROUTE_OFFSET, Val32);
#else
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_HT_ROUTE_OFFSET) | 0x1;
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_HT_ROUTE_OFFSET, Val32);
#endif
#endif

#ifdef LS7A_PHYS48_TO_HT40
	//support 40 bit DMA device otherwise it lost node id
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_HT_ROUTE_OFFSET) & ~(0xff << 8);
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_HT_ROUTE_OFFSET, Val32);
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_HT_ROUTE_OFFSET) | ((0x2 << 8) | (0x6 << 13));
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_HT_ROUTE_OFFSET, Val32);
#endif
	ls7a_debug("config 7A dma route done.\r\n");

#ifndef LS7A_2WAY_CONNECT
	//shut down LS7A HT Hi
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_HT_CLKEN_OFFSET) & ~(0x1 << 1);
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_HT_CLKEN_OFFSET, Val32);
	ls7a_debug("7A HT Hi clock disabled.\r\n");
#endif

#if 1//def  LS7A_UC_ACC
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET + 0x4) | (0x3f << 0);
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET + 0x4, Val32);
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) | (0xef << 24);
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET, Val32);
	ls7a_debug("LS7A uncache accellerator enabled\r\n");
#endif

	//configure 7A pll
	//pcie, gmac, sata/usb
	if (ConfigOnePll(CONF_PLL0_OFFSET + LS7A_CONFBUS_BASE_ADDR, LS7A_PLL_VALUE(80, 8, 16, 12), 0x4)) {
		ls7a_debug("!!!LS7A PLL0 soft configure fail.\r\n");
		return -1;
	}
	//gpu, gmem, dc
	if (ConfigOnePll(CONF_PLL1_OFFSET + LS7A_CONFBUS_BASE_ADDR, (LS7A_VERSION ? LS7A_PLL_VALUE(127, 6, 6, 12) : LS7A_PLL_VALUE(127, 8, 6, 12)), 0x4)) {
		ls7a_debug("!!!LS7A PLL1 soft configure fail.\r\n");
		return -1;
	}
	//flex, node, hda bitclk
	if (ConfigOnePll(CONF_PLL2_OFFSET + LS7A_CONFBUS_BASE_ADDR, LS7A_PLL_VALUE(96, 72, (LS7A_VERSION ? 6 : 6), 100), 0x4)) {
		ls7a_debug("!!!LS7A PLL2 soft configure fail.\r\n");
		return -1;
	}
	//PIX0, default 38.2MHz for x800x600
	//PIX1, default 38.2MHz for x800x600
	if (ConfigOnePll(CONF_PLL3_OFFSET + LS7A_CONFBUS_BASE_ADDR, LS7A_PLL_VALUE(104, 68, 68, 68), 0x4)) {
		ls7a_debug("!!!LS7A PLL3 soft configure fail.\r\n");
		return -1;
	}
	if (ConfigOnePll(CONF_PLL4_OFFSET + LS7A_CONFBUS_BASE_ADDR, LS7A_PLL_VALUE(104, 68, 68, 68), 0x4)) {
		ls7a_debug("!!!LS7A PLL4 soft configure fail.\r\n");
		return -1;
	}
	ls7a_debug("LS7A pll configure done.\r\n");

	//init 7a hardware
	//configure to obey strict HT order
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + 0x414) | (0x7ffff << 0);
	Writel(LS7A_CONFBUS_BASE_ADDR + 0x414, Val32);
	//rewrite pci header default value
	for (i = 0; i <= 0x480; i += 0x40) {
//		Writeb(LS7A_CONFBUS_BASE_ADDR + 0x300c + i, 0x0);
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + 0x300c + i) & 0xffffff00;
		Writel(LS7A_CONFBUS_BASE_ADDR + 0x300c + i, Val32);
		Writel(LS7A_CONFBUS_BASE_ADDR + 0x303c + i, 0x100);
	}
	//rewrite stat prefetch default value
	Writel(LS7A_CONFBUS_BASE_ADDR + 0x454, 0x4708af35);
	//rewrite HEADER TYPE to Multi-function
	Writeb(LS7A_CONFBUS_BASE_ADDR + 0x304e, 0x80);
	Writeb(LS7A_CONFBUS_BASE_ADDR + 0x308e, 0x80);
	Writeb(LS7A_CONFBUS_BASE_ADDR + 0x32ce, 0x80);
	Writeb(LS7A_CONFBUS_BASE_ADDR + 0x330e, 0x80);
	Writeb(LS7A_CONFBUS_BASE_ADDR + 0x334e, 0x80);
	//fix GPU/DC header
	Writel(LS7A_CONFBUS_BASE_ADDR + 0x31c8, 0x04000000);
	Writel(LS7A_CONFBUS_BASE_ADDR + 0x3208, 0x03000000);

	//fix LPC header
	Writel(LS7A_CONFBUS_BASE_ADDR + 0x3448, 0x06010000);
	Writel(LS7A_CONFBUS_BASE_ADDR + 0x3460, 0xfc000000);
	Writel(LS7A_CONFBUS_BASE_ADDR + 0x3464, 0xfd);
	Writel(LS7A_CONFBUS_BASE_ADDR + 0x3450, 0x10002000);
	Writel(LS7A_CONFBUS_BASE_ADDR + 0x3458, 0x12000000);
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + 0x3444) | 0x3;
	Writel(LS7A_CONFBUS_BASE_ADDR + 0x3444, Val32);

	//disable pci scan of MISC and confbus
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + 0x3800) | 0xf;
	Writel(LS7A_CONFBUS_BASE_ADDR + 0x3800, Val32);
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + 0x3878) | 0xf;
	Writel(LS7A_CONFBUS_BASE_ADDR + 0x3878, Val32);
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + 0x3000) | 0xffffffff;
	Writel(LS7A_CONFBUS_BASE_ADDR + 0x3000, Val32);
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + 0x33c0) | 0xffffffff;
	Writel(LS7A_CONFBUS_BASE_ADDR + 0x33c0, Val32);
	//Disable write to RO for IRQ Line bits of device header ---- start
	for (i = 0; i <= 0x90; i += 0x8) {
//		Writew(LS7A_CONFBUS_BASE_ADDR + 0x3800 + i, 0x10);
		Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + 0x3800 + i) & 0xffff0000;
		Val32 = Val32 | 0x10;
		Writel(LS7A_CONFBUS_BASE_ADDR + 0x3800 + i, Val32);
		Writel(LS7A_CONFBUS_BASE_ADDR + 0x3804 + i, 0x10000000); /* Bit 60, PCI header offset 3Ch. Interrupt Line */
	}
	//Special take care of GMEM BAR, clear to 4KB
	//if defined CFG_GMEM, this Register will be reconfigured.
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_GMEM_BAR_MASK_OFFSET, 0xfff);
	Writel(LS7A_CONFBUS_BASE_ADDR + CONF_GMEM_BAR_MASK_OFFSET + 0x4, 0x0);
	//Special take care of GPU-BAR3(64)
	Val32 = Readl(LS7A_CONFBUS_BASE_ADDR + 0x3844) | 0xfc;
	Writel(LS7A_CONFBUS_BASE_ADDR + 0x3844, Val32);
	//Special take care of LPC
	Writel(LS7A_CONFBUS_BASE_ADDR + 0x3888, 0x0);
	//Disable write to part of header ---- end

//change INT and HPET fix address
	Writel(LS7A_CONFBUS_BASE_ADDR + 0x460, (INT_BASE_ADDR | 0x4));
	Writel(LS7A_CONFBUS_BASE_ADDR + 0x464, (HPET_BASE_ADDR | 0x4));
	ls7a_debug("LS7A hardware init done.\r\n");
//3. device configure
	DeviceCfg(PcieInfo);
	ls7a_debug("\r\nLS7A init done.\r\n");

	return 0;
}

int LoongsonHtIntTransInit(void)
{
#ifdef LS3A4000
#ifndef MULTI_CHIP
	u32 NodeNum = 1;
#elif defined (DUAL_CPU)
	u32 NodeNum = 2;
#elif defined (QUAD_CPU)
	u32 NodeNum = 4;
#endif

	s32 i;
	u64 Addr;
	for (i = 0;i < NodeNum;i++) {
		Addr = (((u64)i << 44) | LOONGSON_HT1_INT_TRANS_ADDR);
		Writeq(Addr, 0x400000001fe01140ULL);
	}
#endif
	return 0;
}

int Ls7aDbg(void)
{
#define DBUGADDRESS 0x900000000f000000 // Memory low 240M begain

	u64 StoreVal, Val = 0x5a5aa5a516161616;
	u32 j = 0;

	//Store the memory old val
	StoreVal = Readq(DBUGADDRESS + ((u64)1 << 59)); //Cache Addr 0x98xx

	//Test begain
	Writeq(DBUGADDRESS, Val);
	Writeq(DBUGADDRESS + ((u64)1 << 59), Val);//Cache Addr 0x98xx

	ls7a_debug("Read local memory .. \n");
	if (Readq(DBUGADDRESS) != Val) {
		ls7a_debug("local cache compare fail!\n");
		j++;
	}

	ls7a_debug("Read node0 7A ..\n");
	if (Readq(DBUGADDRESS + 0x00000e0000000000) != Val) {
		ls7a_debug("LS7A > node0 compare fail!\n");
		j++;
	}

#ifdef LS7A_2WAY_CONNECT
	u64 NodeId = 1;

	ls7a_debug("Read node1 7A ..\n");
	if (Readq(DBUGADDRESS + (NodeId << NODE_OFFSET) + 0x00000e0000000000) != Val) {
		ls7a_debug("node1 > LS7A > node0 compare fail!\n");
	}
#endif

	//Recover the memory old val
	Writeq(DBUGADDRESS + ((u64)1 << 59), StoreVal); //Cache Addr 0x98xx
#undef DBUGADDRESS
	if (j != 0)
		return -1;
	else
		return 0;
}

/*
 * Ls7aClearPciePortIrq:
 * Clean the Ls7a internal PCIe prot useless IRQ, to workaround the bug
 * that some PCIe device will creat pseudo interrupt in kernel.
 * This code has some risk that if the PCIe PHY connect device time are
 * not enough, this funciton will lose efficacy.
 */
int Ls7aClearPciePortIrq(Ls7aPcieInfo *PcieInfo)
{
	s32 i;

	for (i = 0; i < (PcieInfo->Num); i++) {
		// This debug print can not delete,if improve the cpu freq the code
		// maybe need fixup.
		ls7a_debug("clear pcie 0x%x\n", PcieInfo -> PcieCfgBuffer[i]);
		// Clear the PCIe internal port useless irq. Otherwise it maybe cause
		// the linked device irq error in kernel.
		LS7A_PCIE_CLEAR_PORT_IRQ(PcieInfo -> PcieCfgBuffer[i]);
	}
	return 0;
}

void ChipsetInit(int NodeNum, int memsz)
{
	int Status = 0;
	Ls7aPcieInfo PcieInfo;

	raw_memsz = (memsz << 10);
	printf("Ls7A1000 Chipset initialize begain\r\n");

	Ls3a7aHtConfigure(NodeNum);
	Status = Ls7aDbg();
	if (Status) {
		ls7a_debug("Test Ls7a access fail!\n");
		while (1);
	}

	PcieInfo.Num = 0;
	Ls7aResourceConfigure(&PcieInfo);
#ifdef LS3A4000
	LoongsonHtIntTransInit(); //kernel must open judge (LOONGSON_CPU_FEATURE_EXT_IOI)
#endif
	//uefi need init gmac phy otherwise kernel network working error
//	Ls7aGmacPhyInit();
	ConfigLs7aWakeUpFunc();
//	Status = SmartFanInit();
	Ls7aClearPciePortIrq(&PcieInfo); // Clean Ls7a internal PCIe useless IRQ

	if (LS7A_VERSION) {
		u32 Val32;
		/*Debug New 7A Func,Drop Rtc voltage*/
		do {
			Val32 = Readl(LS7A_MISC_BASE + LS7A_ACPI_OFFSET + 0x08) | (0x3 << 9);
			Writel(LS7A_MISC_BASE + LS7A_ACPI_OFFSET + 0x08, Val32);
			Val32 = Readl(LS7A_MISC_BASE + LS7A_ACPI_OFFSET + 0x08) & (0x3 << 9);
		} while(!Val32);
	}
	printf("Ls7A1000 Chipset initialize done\n");
}
